#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
int servoValuesInit[7] = {350,242,280,284,480,290,304};

int cmdNum=0;
int cmdValue=0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  
  pwm.begin();
  pwm.setPWMFreq(50);  // Analog servos run at ~60 Hz updates

  while (!Serial) {
  ; // wait for serial port to connect. Needed for Leonardo only
  }
  
  initServo();
}

void loop() {
  
  if (receiveCMD()) {
    Serial.print(cmdNum);
    Serial.print(" ");
    Serial.println(cmdValue);
    
    if ( (cmdNum == -1) && ( cmdValue == 0 ) )
      initServo();
    else if ( (cmdNum>=0) && (cmdNum<=6) )
      pwm.setPWM(cmdNum, 0, cmdValue );
  }
  delay(10);
  
}

void initServo() {
  for (int i=0;i<7;i++) {
    pwm.setPWM(i, 0, servoValuesInit[i] );
  }
}

boolean receiveCMD() {
  if ( Serial.available()>0 ) {
    int a = (char)(Serial.read());
    int b = (unsigned char)(Serial.read())*256 + (unsigned char)(Serial.read());
    
    int aa = (char)(Serial.read());
    int bb = (unsigned char)(Serial.read())*256 + (unsigned char)(Serial.read());
    if ( (a == aa) && ( b==bb ) ) {
      cmdNum = a;
      cmdValue = b;
      return true;
    } else {
      Serial.println("failt!");
      return false;
    }
  } else
    return false;
}
